/*
 * Leg.h
 *
 *  Created on: Feb 19, 2011
 *      Author: Anton
 */
#ifndef LEG_H_
#define LEG_H_
#define LINKS 3
#define LINK1LENGTH .15
#define LINK2LENGTH .381
#define LINK3LENGTH .381
#define LINK1LENGTHCG .07
#define LINK2LENGTHCG .115
#define LINK3LENGTHCG .128
#define LINK1MASS 8.6
#define LINK2MASS 2.6
#define LINK3MASS 1.2




#include "Link.h"
#include "Matrix.h"
#include <vector>
#include <iostream>
using std::vector;
using std::endl;
using std::cout;
using std::cin;


class Leg {
public:
	Leg();
	virtual ~Leg();
	double getUnit() const;
	double getX() const;
	double getY() const;
	double getZ() const;
	void setUnit(double unit);
	void setX(double x);
	void setY(double y);
	void setZ(double z);
	double getDesiredx() const;
	double getDesiredy() const;
	double getDesiredz() const;
	Matrix getI() const;
	void setI(Matrix I);
	void setDesiredx(double desiredx);
	void setDesiredy(double desiredy);
	void setDesiredz(double desiredz);
	Matrix forwardK();
	Link* links;
	Matrix inverseK();
private:
	double unit;
	double x;
	double y;
	double z;
	double desiredx;
	double desiredy;
	double desiredz;
	Matrix I;
};

#endif

